﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace XNAViewerSample.Physics
{
    public interface Constraints
    {
        void SatisfyConstraint();
    }

    /// <summary>
    /// Constrains an object to a certain point in space
    /// </summary>
    public sealed class PointConstraint : Constraints
    {
        #region Fields
        private Vector3 point;
        private SoftBodyObjects sbObject;

        public Vector3 Point
        {
            get { return point; }
            set { point = value; }
        }
        public SoftBodyObjects SbModel
        {
            get { return sbObject; }
            set { sbObject = value; }
        }
        #endregion

        #region Constructor
        public PointConstraint(Vector3 point, SoftBodyObjects sbObject)
        {
            this.point = point;
            this.sbObject = sbObject;
        }
        #endregion

        #region Methods
        public void SatisfyConstraint()
        {
            //move to goal position
            sbObject.CurrPosition = point;
        }
        #endregion
    }

    /// <summary>
    /// Constrains two objects to maintain a specified distance
    /// </summary>
    public sealed class LengthConstraint : Constraints
    {
        #region Fields
        private float length;
        private SoftBodyObjects sbObj1;
        private SoftBodyObjects sbObj2;

        public float Length
        {
            get { return length; }
            set { length = value; }
        }
        #endregion

        #region Constructor
        public LengthConstraint(float length, SoftBodyObjects sbObj1, SoftBodyObjects sbObj2)
        {
            this.length = length;
            this.sbObj1 = sbObj1;
            this.sbObj2 = sbObj2;
        }
        #endregion

        #region Methods
        Vector3 direction;
        float currentLength;
        Vector3 moveVector;
        public void SatisfyConstraint()
        {
            //calculate direction
            direction = sbObj2.CurrPosition - sbObj1.CurrPosition;

            //calculate current length
            currentLength = direction.Length();

            //check for zero vector
            if (direction != Vector3.Zero)
            {
                //normalize direction vector
                direction.Normalize();

                //move to goal positions
                moveVector = 0.5f * (currentLength - length) * direction;
                sbObj1.CurrPosition += moveVector;
                sbObj2.CurrPosition += -moveVector;
            }
        }
        #endregion
    }
}
